import os

from ament_index_python.packages import get_package_share_directory
from launch_ros.actions import Node
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration


def generate_launch_description():
    # 获取各功能包路径
    amcl_pkg = get_package_share_directory("mycar_localization")
    nav2_pkg = get_package_share_directory("mycar_navigation2")
    go2_desc_pkg = get_package_share_directory("go2_description")
    go2_driver_pkg = get_package_share_directory("go2_driver_py")
    lslidar_trans_pkg = get_package_share_directory("lslidar_driver")
    go2_slam_pkg = get_package_share_directory("go2_agiros_slam")

    # 声明启动参数
    use_rviz_arg = DeclareLaunchArgument(
        'use_rviz',
        default_value='true',
        description='Whether to launch RViz'
    )

    # 1. 首先启动URDF（建立TF树）
    display_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(go2_desc_pkg, "launch", "display.launch.py")
        ),
        launch_arguments={"use_joint_state_publisher": "false"}.items()
    )

    # 2. 启动机器人驱动（提供里程计）
    go2_driver_node = Node(
        package="go2_driver_py",
        executable="driver",
        parameters=[os.path.join(go2_driver_pkg, "params", "go2_driver.yaml")],
        output="screen",
        emulate_tty=True,
    )

    # 3. 启动雷达转换驱动
    lslidar_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(lslidar_trans_pkg, "launch", "lsn10p_launch.py")
        )
    )

    # 4. 速度桥接节点
    go2_twist_node = Node(
        package="go2_twist_bridge_py",
        executable="twist_bridge",
        output="screen",
    )

    # 5. 定位节点
    amcl_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(amcl_pkg, 'launch', 'mycar_loca.launch.py')
        )
    )

    # 6. 导航节点
    nav2_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(nav2_pkg, 'launch', 'nav2.launch.py')
        )
    )

    # 7. RViz可视化
    rviz_node = Node(
        package="rviz2",
        executable="rviz2",
        name="rviz2",
        arguments=["-d", os.path.join(go2_slam_pkg, "rviz", "go2_super.rviz")],
        condition=IfCondition(LaunchConfiguration("use_rviz")),
        output="screen"
    )

    ld = LaunchDescription()
    # 首先添加参数声明
    ld.add_action(use_rviz_arg)
    
    # 按依赖顺序添加节点
    ld.add_action(display_launch)      # TF树和机器人描述
    ld.add_action(go2_driver_node)     # 机器人驱动和里程计
    ld.add_action(lslidar_launch)      # 传感器数据处理
    ld.add_action(go2_twist_node)      # 速度转换
    ld.add_action(amcl_launch)         # 定位
    ld.add_action(nav2_launch)         # 导航
    ld.add_action(rviz_node)           # 可视化
    
    return ld
